#include "SDM.h"
#include "Polyhedron.h"
#include "AABB.h"
#include <math.h>

#define POS_INF	1e10

SDM::SDM(void)
{
}


SDM::~SDM(void)
{
}

BruteForceSDM::BruteForceSDM( void )
{

}

BruteForceSDM::BruteForceSDM( Polyhedron* p )
{
	poly = p;
}

BruteForceSDM::~BruteForceSDM( void )
{

}

Value BruteForceSDM::getEvaluation( Vec3d p, bool compute_normal )
{
	double sqrDist;
	Vec3d closestPt, normal;

	poly->getSqrDistance(p, &sqrDist, &closestPt, & normal);

	Value v;
	v.p = p;

	if (dot((p - closestPt), normal) < 0)
		v.d = -sqrt(sqrDist);
	else
		v.d = sqrt(sqrDist);

	v.n = normal;

	return v;
}

GridSDM::GridSDM( void )
{

}

GridSDM::GridSDM( Polyhedron* poly, double cellSize )
{
	this->cellSize = cellSize;
	this->invCellSize = 1.0 / cellSize;
	bfsdm = new BruteForceSDM(poly);

	bbox = new AABB();

	for (int i=0; i<poly->vertCount; i++)
	{
		Vert v = poly->vertList[i];

		bbox->Expand(Vec3d(v.x, v.y, v.z));
	}

	Vec3d extents = bbox->p1 - bbox->p0;

	w = (int)ceil(extents[0] * invCellSize) + 3;
	h = (int)ceil(extents[1] * invCellSize) + 3;
	d = (int)ceil(extents[2] * invCellSize) + 3;

	grid = (double*)malloc(w * h * d * sizeof(double));
	gridInitialized = (bool*)malloc(w * h * d * sizeof(bool));

	for (int i=0; i<w*h*d; i++)
	{
		gridInitialized[i] = false;
	}
}

GridSDM::~GridSDM( void )
{

}

double GridSDM::getGridData( int x, int y, int z )
{
	return grid[x * h * d + y * d + z];
}

void GridSDM::setGridData( int x, int y, int z, double v )
{
	grid[x * h * d + y * d + z] = v;
}

bool GridSDM::getGridInitData( int x, int y, int z )
{
	return gridInitialized[x * h * d + y * d + z];
}

void GridSDM::setGridInitData( int x, int y, int z, bool v )
{
	gridInitialized[x * h * d + y * d + z] = v;
}

double GridSDM::getGridValue( int x, int y, int z )
{
	x++; y++; z++;

	if (!getGridInitData(x, y, z))
	{
		setGridData(x, y, z, bfsdm->getEvaluation(Vec3d(x-1, y-1, z-1) * cellSize + bbox->p0, true).d);
		setGridInitData(x, y, z, true);		
	}

	return getGridData(x, y, z);
}

Value GridSDM::getEvaluation( Vec3d p, bool compute_normal )
{
	Value v;
	v.p = p;
	v.n = Vec3d(0, 0, 1);
	v.d = POS_INF;

	// check against bbox
	if (!bbox->Contains(p)) return v;

	Vec3d ptRel = (p - bbox->p0) * invCellSize;
	int x = (int)floor(ptRel[0]);
	int y = (int)floor(ptRel[1]);
	int z = (int)floor(ptRel[2]);
	Vec3d ptOff = ptRel - Vec3d(x, y, z);

	// trilerp to get the value
	v.d = 
		((getGridValue(x+0,y+0,z+0) * (1-ptOff[0]) + getGridValue(x+1,y+0,z+0) * (0+ptOff[0])) * (1-ptOff[1]) + 
		(getGridValue(x+0,y+1,z+0) * (1-ptOff[0]) + getGridValue(x+1,y+1,z+0) * (0+ptOff[0])) * (0+ptOff[1])) * (1-ptOff[2]) +
		((getGridValue(x+0,y+0,z+1) * (1-ptOff[0]) + getGridValue(x+1,y+0,z+1) * (0+ptOff[0])) * (1-ptOff[1]) +
		(getGridValue(x+0,y+1,z+1) * (1-ptOff[0]) + getGridValue(x+1,y+1,z+1) * (0+ptOff[0])) * (0+ptOff[1])) * (0+ptOff[2]);

	if (compute_normal)
	{
		// bilerp to get the gradient in each dimension
		double dfdx = 0;
		double debug;

		debug = getGridValue(x+1,y+0,z+0);
		debug = getGridValue(x+0,y+0,z+0);
		dfdx += (getGridValue(x+1,y+0,z+0) - getGridValue(x+0,y+0,z+0)) * (1-ptOff[1]) * (1-ptOff[2]);
		dfdx += (getGridValue(x+1,y+1,z+0) - getGridValue(x+0,y+1,z+0)) * (0+ptOff[1]) * (1-ptOff[2]);
		dfdx += (getGridValue(x+1,y+0,z+1) - getGridValue(x+0,y+0,z+1)) * (1-ptOff[1]) * (0+ptOff[2]);
		dfdx += (getGridValue(x+1,y+1,z+1) - getGridValue(x+0,y+1,z+1)) * (0+ptOff[1]) * (0+ptOff[2]);

		double dfdy = 0;
		dfdy += (getGridValue(x+0,y+1,z+0) - getGridValue(x+0,y+0,z+0)) * (1-ptOff[2]) * (1-ptOff[0]);
		dfdy += (getGridValue(x+0,y+1,z+1) - getGridValue(x+0,y+0,z+1)) * (0+ptOff[2]) * (1-ptOff[0]);
		dfdy += (getGridValue(x+1,y+1,z+0) - getGridValue(x+1,y+0,z+0)) * (1-ptOff[2]) * (0+ptOff[0]);
		dfdy += (getGridValue(x+1,y+1,z+1) - getGridValue(x+1,y+0,z+1)) * (0+ptOff[2]) * (0+ptOff[0]);

		double dfdz = 0;
		dfdz += (getGridValue(x+0,y+0,z+1) - getGridValue(x+0,y+0,z+0)) * (1-ptOff[0]) * (1-ptOff[1]);
		dfdz += (getGridValue(x+1,y+0,z+1) - getGridValue(x+1,y+0,z+0)) * (0+ptOff[0]) * (1-ptOff[1]);
		dfdz += (getGridValue(x+0,y+1,z+1) - getGridValue(x+0,y+1,z+0)) * (1-ptOff[0]) * (0+ptOff[1]);
		dfdz += (getGridValue(x+1,y+1,z+1) - getGridValue(x+1,y+1,z+0)) * (0+ptOff[0]) * (0+ptOff[1]);

		v.n = Vec3d(1,0,0) * dfdx + Vec3d(0,1,0) * dfdy + Vec3d(0,0,1) * dfdz;

		v.n = normalized(v.n);
	}

	return v;
}
